#include <Servo.h>
#include <math.h>
#define PI 3.14159265
Servo S1;
Servo S2;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=179,b=145,c=53,e=90;//wymiary w mm

void check(float,float,float);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2


void setup() {
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);//tuuuuuu
  S2.attach(10);//tuuuuuu
  S2.write(90);
  S1.write(90);
  delay(10);
  pinMode(12,OUTPUT);
  digitalWrite(13,LOW);
}
 
void loop() {
  if(digitalRead(A0)==0)
  {
    digitalWrite(12,HIGH);
    int i = S2.read();
    int j = S1.read();

    int nom;
    float katServ[2];
    float x=0,h=0;
    
    
    Serial.print("\nPodaj odl:");
    delay(1000);
    while (Serial.available()==0)
    
    delay(400);
    x = Serial.parseFloat();
    h = Serial.parseFloat();
    Serial.print(x);
    Serial.print(h);
    
    
    if (Serial.available()>0)
    {Serial.print("\njest if\n:");
    /*Serial.print("\nPodaj wys:");
    delay(50);
    while(Serial.available()==0)
    h = Serial.parseInt();
    Serial.print(h);*/
    
    if((x>0) && (h>0))
    {
      check(x,h,katServ);
    
    if(i<katServ[1]){
     for(i;i<=katServ[1];i++)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(i>katServ[1]){
     for(i;i>=katServ[1];i--)
        {
        S2.write(i);
        delay(50);
        } //koniec fora 
        }
    if(j<katServ[0]){
    for(j;j<=katServ[0];j++)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
    if(j>katServ[0]){
    for(j;j>=katServ[0];j--)
        {
        S1.write(j);
        delay(50);
        } //koniec fora
        }
   
    }
    delay(1000);
    digitalWrite(12,LOW);
    }//przycisk
  }
    //if(digitalRead(A0)==0)
    //goto poczatek;
 }


void check(float odlx,float wysy,float katServ[])
{
    int i,j;
    float h,X,D,y1,y2,kat1,kat2,odl,roznica,alfa,beta,odwrot;
    roznica=100;
    

    for (i=S1min;i<=S1max;i++)
    {
        for (j=S2min; j<=S2max;j++)
        {   
          odwrot=180-j;
            alfa=abs(i-90);
            if(i>=90)
            {   if((j+alfa)<180)
                beta=abs((odwrot-90+alfa));
                else
                beta=odwrot-180+alfa;
                
            }
            else
            {   
                if(odwrot<=alfa)
                beta=abs(alfa-odwrot);
                else
                beta=abs((odwrot-90-alfa));
                
            }
          
       
              //Serial.print("\talfa = ");Serial.print(alfa);
              //Serial.print("\tbeta = ");Serial.print(beta);
      kat1= alfa/180*PI;//kąt w radianach
            kat2= beta/180*PI;
           //Serial.print("\tarad = ");Serial.print(kat1);
             //Serial.print("\tbrad = ");Serial.print(kat2);
          
            if(((i>=90) && ((odwrot+alfa)>=180)) || ((i<90) && (odwrot<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && ((odwrot+alfa)>=180))
            h=a+y1+y2-e;
            else if((i>=90) && ((odwrot+alfa)<180))
            h=a+y1-y2-e;
            else if((i<90) && (odwrot<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (odwrot>alfa))
            h=a-y1-y2-e;

            if(((odwrot>=(90-alfa)) && (i>=90)) || ((i<90) &&  (odwrot>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if ((roznica>=(abs(odlx-odl)+abs(wysy-h))) && (wysy<=h))
            { Serial.print(".  if.  ");
                roznica=(abs(odlx-odl)+abs(wysy-h));//wazne
                katServ[0]=i+10; Serial.print("S1 == "); Serial.print(i);
                katServ[1]=j; Serial.print(" S2 == "); Serial.print(j);
               
              Serial.print("\troz = "); Serial.print(roznica); 
              Serial.print("\talfa = ");Serial.print(alfa);
              Serial.print("\tbeta = ");Serial.print(beta);
              Serial.print("\todl = ");Serial.print(odl);
              Serial.print("\th = ");Serial.print(h);
             // Serial.print("\tarad = ");Serial.print(kat1);
             // Serial.print("\tbrad = ");Serial.print(kat2);
              
              Serial.print("\n");
            }

        }
    }

}//koniec check